#!/usr/bin/env python
# -*- coding: UTF-8 -*-
"""
@Project ：pid_control_example
@File ：PIDController.py
@Author ：李涵彬 (优化版本)
@Date ：2024/5/14 下午9:37
"""

import time
from typing import Optional, Callable, Union


class PIDController:
	"""
	PID 控制器类,用于实现比例-积分-微分控制算法。

	Attributes:
	- output: 当前的PID控制器输出值
	- int_error: 积分误差累计值
	- last_error: 上一次的误差值
	- DTerm: 微分项
	- ITerm: 积分项
	- PTerm: 比例项
	- SetPoint: 目标值
	- Kp: 比例系数
	- Ki: 积分系数
	- Kd: 微分系数
	- sample_time: 采样时间段
	- current_time: 当前时间
	- last_time: 上一次更新的时间
	- clamp_output: 限制输出函数
	- clamp_integral: 限制积分项函数
	"""

	def __init__(self,
				 P: float = 0.2,
				 I: float = 0.0,
				 D: float = 0.0,
				 current_time: Optional[float] = None,
				 clamp_output: Optional[Callable[[float], float]] = None,
				 clamp_integral: Optional[Callable[[float], float]] = None) -> None:
		"""
		初始化PID控制器。

		:param P: P算法超参数,预期为正数。
		:param I: I算法超参数,预期为正数。
		:param D: D算法超参数,预期为正数。
		:param current_time: 当前时刻,如果为None则使用调用时的当前时间。
		:param clamp_output: 限制输出值的函数,可选。
		:param clamp_integral: 限制积分项的函数,可选。
		"""
		# 参数校验
		if not all(isinstance(param, (int, float)) and param >= 0 for param in [P, I, D]):
			raise ValueError("P, I, D 参数必须为非负数。")

		self.output = None
		self.int_error = None
		self.last_error = None
		self.DTerm = None
		self.ITerm = None
		self.PTerm = None
		self.SetPoint = None
		self.Kp = P
		self.Ki = I
		self.Kd = D

		# 采样时间段
		self.sample_time = 0.00
		# 使用默认的当前时间,如果提供了current_time则使用提供的值
		self.current_time = current_time if current_time is not None else time.time()
		self.last_time = self.current_time

		# 输出限制和积分限制函数
		self.clamp_output = clamp_output
		self.clamp_integral = clamp_integral

		self.clear()

	def clear(self) -> None:
		"""
		重置控制器状态。
		"""
		self.output = None
		self.int_error = 0.0
		self.last_error = None
		self.DTerm = 0.0
		self.ITerm = 0.0
		self.PTerm = 0.0
		self.SetPoint = None

	def update(self, process_value: float, setpoint: float) -> float:
		"""
		根据当前过程值和目标值更新PID控制器的输出。

		:param process_value: 当前过程值
		:param setpoint: 目标值
		:return: PID控制器的输出值
		"""
		self.current_time = time.time()
		error = setpoint - process_value
		dt = self.current_time - self.last_time

		if dt <= 0:
			raise ValueError("采样时间间隔必须大于0。")

		# 计算比例项
		self.PTerm = self.Kp * error

		# 积分项
		self.ITerm += self.Ki * error * dt
		if self.clamp_integral:
			self.ITerm = self.clamp_integral(self.ITerm)

		# 微分项
		if self.DTerm is None or dt == 0:
			self.DTerm = 0
		else:
			self.DTerm = self.Kd * (error - self.last_error) / dt

		# 更新输出值和上一次的误差
		self.output = self.PTerm + self.ITerm + self.DTerm
		if self.clamp_output:
			self.output = self.clamp_output(self.output)
		self.last_error = error
		self.last_time = self.current_time

		return self.output

	def set_integral_limits(self, clamp_integral: Callable[[float], float]) -> None:
		"""
		设置积分项的限制函数。

		:param clamp_integral: 限制积分项的函数。
		"""
		self.clamp_integral = clamp_integral

	def set_output_limits(self, clamp_output: Callable[[float], float]) -> None:
		"""
		设置输出值的限制函数。

		:param clamp_output: 限制输出值的函数。
		"""
		self.clamp_output = clamp_output


# 以下是一些示例限制函数
def clamp_integral_symmetric(value: float, max_value: float) -> float:
	"""
	限制积分项在对称范围内。

	:param value: 积分项值
	:param max_value: 最大允许值
	:return: 限制后的积分项值
	"""
	return max(-max_value, min(max_value, value))


def clamp_output_positive(value: Union[int, float]) -> float:
	"""
	限制输出值为非负数。

	:param value: 输出值
	:return: 限制后的输出值
	"""
	return max(0.0, value)
